#!/usr/bin/env python

# Copyright (c) 2018, John Morris
# All rights reserved.

# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#     * Redistributions of source code must retain the above
#       copyright notice, this list of conditions and the following
#       disclaimer.
#     * Redistributions in binary form must reproduce the above
#       copyright notice, this list of conditions and the following
#       disclaimer in the documentation and/or other materials
#       provided with the distribution.
#     * Neither the name of the <organization> nor the names of its
#       contributors may be used to endorse or promote products
#       derived from this software without specific prior written
#       permission.

# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
# <COPYRIGHT HOLDER> BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
# USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
# OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGE.

# coding=utf-8

import rospy
from std_msgs.msg import Bool, Float64, UInt32, Int32
import hal
import attr


class HalIOException(RuntimeError):
    pass


@attr.s
class HalPin(object):
    hal_type = attr.ib()
    hal_dir = attr.ib()
    msg_type = attr.ib()
    name = attr.ib()
    sub_topic = attr.ib(default='')
    pub_topic = attr.ib(default='')
    hal_pin = attr.ib(default=None)
    sub = attr.ib(default=None)
    pub = attr.ib(default=None)


class HalIO(object):
    cname = 'hal_io'
    pin_type_map = dict(
        BIT=hal.HAL_BIT,
        FLOAT=hal.HAL_FLOAT,
        U32=hal.HAL_U32,
        S32=hal.HAL_S32,
    )
    msg_type_map = dict(
        BIT=Bool,
        FLOAT=Float64,
        U32=UInt32,
        S32=Int32,
    )
    pin_dir_map = dict(
        IN=hal.HAL_IN,
        OUT=hal.HAL_OUT,
        IO=hal.HAL_IO,
    )

    def __init__(self):
        rospy.loginfo("Initializing '%s' component" % self.cname)

        # Create ROS node
        rospy.init_node(self.cname)

        # Publisher update rate in Hz
        self.update_rate = rospy.get_param('%s/update_rate' % self.cname, 10)
        self.rate = rospy.Rate(self.update_rate)
        rospy.loginfo("Publish update rate = %.1f" % self.update_rate)

        # Load pin configuration from ROS param server
        self.pin_config = rospy.get_param('{}/pins'.format(self.cname), dict())
        if not self.pin_config:
            raise HalIOException(
                'No pins defined in parameter "%s"' % self.cname)

        # Init HAL component and pin data dict
        self.comp = hal.component(self.cname)
        self.pins = dict()

        for pin_name, pin_data in self.pin_config.items():
            if len(pin_data) < 2:
                raise HalIOException(
                  'Must define HAL type and direction for pin "{}"'.format(pin_name)
                )
            type_str, dir_str = pin_data[0], pin_data[1]

            try:
                pin = self.pins[pin_name] = HalPin(
                    name=pin_name,
                    hal_type=self.pin_type_map[type_str],
                    hal_dir=self.pin_dir_map[dir_str],
                    msg_type=self.msg_type_map[type_str],
                )
            except KeyError:
                raise HalIOException(
                  'Unknown type "{}" or dir "{}" for pin "{}"'.format(
                    type_str, dir_str, pin_name,
                  )
                )

            # set ROS topics
            if len(pin_data) == 2:
                pin.pub_topic = pin.sub_topic = '{}/{}'.format(self.cname, pin_name)
            elif len(pin_data) == 3:
                pin.pub_topic = pin.sub_topic = pin_data[2]
            else:
                pin.pub_topic = pin_data[2]
                pin.sub_topic = pin_data[3]

            rospy.loginfo('Creating {} pin "{}" type "{}"'.format(
                {hal.HAL_IN: 'input', hal.HAL_OUT: 'output'}.get(pin.hal_dir, 'input/output'),
                pin_name,  pin_data[0],
            ))
            pin.hal_pin = self.comp.newpin(pin_name, pin.hal_type, pin.hal_dir)

            if pin.hal_dir in (hal.HAL_OUT, hal.HAL_IO):
                rospy.loginfo('Creating subscriber on topic "{}"'.format(pin.sub_topic))
                pin.sub = rospy.Subscriber(pin.sub_topic, pin.msg_type, self.subscriber_cb, pin)

            rospy.loginfo('Creating publisher on topic "{}"'.format(pin.pub_topic))
            pin.pub = rospy.Publisher(pin.pub_topic, pin.msg_type, queue_size=1, latch=True)

        # Finish initialization
        self.comp.ready()
        rospy.loginfo(
            "User component '%s' ready" % self.cname)

    def subscriber_cb(self, msg, pin):
        # Sanity check
        if type(msg) is not pin.msg_type:
            raise HalIOException(
                "subscriber_cb:  Received incorrect message type '%s' for "
                "pin '%s' of msg type '%s'" %
                (type(msg), pin.name, pin.msg_type))

        # Set pin value from message data
        # rospy.logdebug(
        #     "subscriber_cb:  Setting pin '%s' to '%s' from msg type '%s'" %
        #     (pin['name'], msg.data, type(msg)))
        self.comp[pin.name] = msg.data

    def publish_pins(self):
        for pin_name, pin in self.pins.items():
            # Publish pin values
            # rospy.logdebug(
            #     "publish_pins:  Publishing pin '%s' value '%s'" %
            #     (pin_name, self.comp[pin_name]))
            pin.pub.publish(self.comp[pin_name])

    def run(self):
        # Loop until shutdown, publishing pins and sleeping
        while not rospy.is_shutdown():
            self.publish_pins()
            self.rate.sleep()


if __name__ == '__main__':
    # Create and name node
    io_loop = HalIO()
    try:
        io_loop.run()
    except rospy.ROSInterruptException:
        pass
    except HalIOException as e:
        rospy.logfatal(e.message)

# Because file has no .py extension, emacs pics up indent_size = 2
# from .editorconfig
#
# Local Variables:
# python-indent-offset: 4
# End:
